package com.guazi.trackingtest.cal;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.support.annotation.Nullable;
import android.view.View;
import android.widget.Button;
import android.widget.TextView;
import android.widget.Toast;

import com.guazi.trackingtest.PackData;
import com.guazi.trackingtest.R;
import com.guazi.trackingtest.Sender;
import com.guazi.trackingtest.cal.data.Point3D;
import com.guazi.trackingtest.cal.op.Adjust;
import com.guazi.trackingtest.cal.op.CalculateGyr;
import com.guazi.trackingtest.cal.op.CalculateLAcc;
import com.guazi.trackingtest.cal.op.Calibration;
import com.guazi.trackingtest.cal.op.Correction;
import com.guazi.trackingtest.cal.op.LinearWeightedMovingAverage;
import com.guazi.trackingtest.filter.Kalman;

import java.io.IOException;

import butterknife.BindView;
import butterknife.ButterKnife;

/**
 * Created by lqy on 2018/3/22.
 */

public class CalculateActivity extends Activity implements SensorEventListener {

    private final static double NS2S = 1000000000.0D;

    private SensorManager mSensorManager;
    private Sensor mGyr;//陀螺仪
    private Sensor mLAcc;//线性加速计（已经去除重力的影响）

    ////////////////////用于计算初始方位
//    private Sensor mAcc;
    private Sensor mMag;
    private boolean calRotate = false;
    float[] accelerometerValues;
    float[] magneticValues;
    float[] mR = new float[9];
    float[] rotateValues = new float[3];
    float[] accValues = new float[3];
    private int rotate_cnt;
    private int acc_cnt;

    private SensorEventListener mListener = new SensorEventListener() {
        @Override
        public void onSensorChanged(SensorEvent event) {
            if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
//                if (acc_cnt < cal_len_1) {
//                    ++acc_cnt;
//                    accValues[0] += event.values[0];
//                    accValues[1] += event.values[1];
//                    accValues[2] += event.values[2];
//                    if (acc_cnt == cal_len_1) {
//                        accelerometerValues = new float[3];
//                        accelerometerValues[0] = accValues[0] / cal_len_1;
//                        accelerometerValues[1] = accValues[1] / cal_len_1;
//                        accelerometerValues[2] = accValues[2] / cal_len_1;
//                        calRotate();
//                    }
//                }
            } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
                if (rotate_cnt < cal_len_1) {
                    ++rotate_cnt;
                    rotateValues[0] += event.values[0];
                    rotateValues[1] += event.values[1];
                    rotateValues[2] += event.values[2];
                    if (rotate_cnt == cal_len_1) {
                        magneticValues = new float[3];
                        magneticValues[0] = rotateValues[0] / cal_len_1;
                        magneticValues[1] = rotateValues[1] / cal_len_1;
                        magneticValues[2] = rotateValues[2] / cal_len_1;
                        calRotate();
                    }
                }
            }
        }

        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {

        }
    };
    ///////////////////////////////////////

    @BindView(R.id.main_ac_error_detect_tv)
    TextView mErrorDetectTv;
    @BindView(R.id.main_ac_reset_btn)
    Button mRestBtn;

    private Sender mSender;

    private Point3D lastPosition;
    private Point3D lastVelocity;
    private Point3D lastAdjustResult;
    private Point3D lastRotationAngle;
    private Point3D lastRotationValue;
    private long lastTimeStampLinear;
    private long lastTimeStampRotation;

    private Point3D flag = new Point3D(0, 0, 0);

    private int count;

    private int cnt_len;
    private int cal_len;
    private int cal_cnt;
    private Point3D cal_val;

    private int cal_len_1;
    private int cal_cnt_1;
    private Point3D cal_val_1;


    private Calibration calibration;

    private LinearWeightedMovingAverage lAccX;
    private LinearWeightedMovingAverage lAccY;
    private LinearWeightedMovingAverage lAccZ;

    private LinearWeightedMovingAverage gyrX;
    private LinearWeightedMovingAverage gyrY;
    private LinearWeightedMovingAverage gyrZ;

    private Kalman mKalman = new Kalman();

    @Override
    protected void onCreate(@Nullable Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        ButterKnife.bind(this);

        mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
        if (mSensorManager != null) {
            mGyr = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
            mLAcc = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
//            mAcc = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
            mMag = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        }

        initJsSender();

        reset(null);

        mSensorManager.registerListener(this, mLAcc, SensorManager.SENSOR_DELAY_GAME);
        mSensorManager.registerListener(this, mGyr, SensorManager.SENSOR_DELAY_GAME);
    }

    @Override
    protected void onResume() {
        super.onResume();

    }

    @Override
    protected void onStop() {
        super.onStop();
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        mSensorManager.unregisterListener(this);
        if (mSender != null) {
            mSender.stop();
        }
    }

    @Override
    public void onSensorChanged(SensorEvent event) {
        if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
            if (cal_cnt < cal_len) {
                ++cal_cnt;
                cal_val.x += event.values[0];
                cal_val.y += event.values[1];
                cal_val.z += event.values[2];

                accValues[0] += event.values[0];
                accValues[1] += event.values[1];
                accValues[2] += event.values[2];

                if (cal_cnt == cal_len) {
                    cal_val.x /= cal_len;
                    cal_val.y /= cal_len;
                    cal_val.z /= cal_len;
                    calibration.linearValue.setPoint(cal_val);
                    mErrorDetectTv.setVisibility(View.VISIBLE);
                    mErrorDetectTv.setText(mErrorDetectTv.getText().toString() + "\n静止时加速度（即误差）\nx:" + cal_val.x + "\ny:" + cal_val.y + "\nz:" + cal_val.z);
//                    calRotate();

                    accelerometerValues = new float[3];
                    accelerometerValues[0] = accValues[0] / cal_len_1;
                    accelerometerValues[1] = accValues[1] / cal_len_1;
                    accelerometerValues[2] = accValues[2] / cal_len_1;
                    calRotate();
                }
            } else {
                runLinear(new Point3D(event.values[0], event.values[1], event.values[2]), event.timestamp);
            }
        } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
            if (cal_cnt_1 < cal_len_1) {
                ++cal_cnt_1;
                cal_val_1.x += event.values[0];
                cal_val_1.y += event.values[1];
                cal_val_1.z += event.values[2];
                if (cal_cnt_1 == cal_len_1) {
                    cal_val_1.x /= cal_len_1;
                    cal_val_1.y /= cal_len_1;
                    cal_val_1.z /= cal_len_1;
                    calibration.rotationValue.setPoint(cal_val_1);
                    mErrorDetectTv.setVisibility(View.VISIBLE);
                    mErrorDetectTv.setText(mErrorDetectTv.getText().toString() + "\n静止时角速度（即误差）\nx:" + cal_val_1.x + "\ny:" + cal_val_1.y + "\nz:" + cal_val_1.z);
                    calRotate();
                }
            } else {
                runRotation(new Point3D(event.values[0], event.values[1], event.values[2]), event.timestamp);
            }
        }
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
    }

    public void reset(View view) {
        mErrorDetectTv.setText("误差测量完成");
        mErrorDetectTv.setVisibility(View.GONE);

        Initialization.init();
        lastPosition = Initialization.position;
        lastVelocity = Initialization.velocity;
        lastAdjustResult = Initialization.linearValue;
        lastRotationValue = Initialization.rotationValue;
        lastRotationAngle = Initialization.rotationAngle;
        lastTimeStampLinear = 0;
        lastTimeStampRotation = 0;

        int ma_len = 6;
        cnt_len = 20;
        count = 0;
        cal_len = 100;
        cal_cnt = 0;
        cal_val = new Point3D();
        cal_len_1 = 100;
        cal_cnt_1 = 0;
        cal_val_1 = new Point3D();

        calibration = new Calibration();
        lAccX = new LinearWeightedMovingAverage(ma_len);
        lAccY = new LinearWeightedMovingAverage(ma_len);
        lAccZ = new LinearWeightedMovingAverage(ma_len);
        gyrX = new LinearWeightedMovingAverage(ma_len);
        gyrY = new LinearWeightedMovingAverage(ma_len);
        gyrZ = new LinearWeightedMovingAverage(ma_len);

        rotate_cnt = 0;
        acc_cnt = 0;
        calRotate = false;
        magneticValues = null;
        accelerometerValues = null;
        rotateValues = new float[3];
        accValues = new float[3];
//        mSensorManager.registerListener(mListener, mAcc, SensorManager.SENSOR_DELAY_GAME);
        mSensorManager.registerListener(mListener, mMag, SensorManager.SENSOR_DELAY_GAME);
        mSender.addPackData(new PackData(1, 1, 0, 0, 0));
    }

    private void runRotation(Point3D rotationValue, long timeStamp) {

        if (lastTimeStampRotation == 0) {
            lastTimeStampRotation = timeStamp;
            return;
        }

        double timeInterval = (timeStamp - lastTimeStampRotation) / NS2S;

        // calibration
        Point3D rotationCorrectionResult = Correction.RotationCorrection(rotationValue, calibration.rotationValue);
//        if(!mKalman.isInitialised()){
//            mKalman.setNoise(calibration.rotationValue);
//        }
//        Point3D rotationCorrectionResult = mKalman.filter(Correction.RotationCorrection(rotationValue, calibration.rotationValue));


        // moving average
        Point3D rotationAfterFilter = new Point3D();
        gyrX.pushValue(rotationCorrectionResult.x);
        rotationAfterFilter.x = gyrX.getAverageValue();
        gyrY.pushValue(rotationCorrectionResult.y);
        rotationAfterFilter.y = gyrY.getAverageValue();
        gyrZ.pushValue(rotationCorrectionResult.z);
        rotationAfterFilter.z = gyrZ.getAverageValue();


        lastRotationAngle = CalculateGyr.update(lastRotationAngle, lastRotationValue, rotationAfterFilter, timeInterval);
        lastRotationValue = rotationAfterFilter;
        lastTimeStampRotation = timeStamp;
    }

    private void runLinear(Point3D accValue, long timeStamp) {
        if (lastTimeStampLinear == 0) {
            lastTimeStampLinear = timeStamp;
            return;
        }

        double timeInterval = (timeStamp - lastTimeStampLinear) / NS2S;

//        Point3D accCorrectionResult = Correction.LinearCorrection(accValue, calibration.linearValue);
        Point3D accCorrectionResult = accValue;

        Point3D accAfterFilter = new Point3D();

//        if (Math.abs(accCorrectionResult.x) < 0.1)
//            accCorrectionResult.x = 0;
//        if (Math.abs(accCorrectionResult.y) < 0.1)
//            accCorrectionResult.y = 0;
//        if (Math.abs(accCorrectionResult.z) < 0.1)
//            accCorrectionResult.z = 0;

        lAccX.pushValue(accCorrectionResult.x);
        lAccY.pushValue(accCorrectionResult.y);
        lAccZ.pushValue(accCorrectionResult.z);
        accAfterFilter.x = lAccX.getAverageValue();
        accAfterFilter.y = lAccY.getAverageValue();
        accAfterFilter.z = lAccZ.getAverageValue();

        Point3D adjustResult;
        Point3D velocity = new Point3D();

        if (Math.abs(accAfterFilter.x - flag.x) > 0.1 || Math.abs(accAfterFilter.y - flag.y) > 0.1 || Math.abs(accAfterFilter.z - flag.z) > 0.1) {

            flag.setPoint(accAfterFilter);

            adjustResult = Adjust.adjust(accAfterFilter, lastRotationAngle);
            velocity = CalculateLAcc.calculateVelocity(lastVelocity, lastAdjustResult, adjustResult, timeInterval);
            Point3D computationResult = CalculateLAcc.calculateDisplacement(lastVelocity, velocity, timeInterval);
            Point3D position = CalculateLAcc.calculatePosition(lastPosition, computationResult);
            mSender.addPackData(new PackData(0, 0, position.x, position.y, position.z));

            lastTimeStampLinear = timeStamp;
            lastVelocity = velocity;
            lastAdjustResult = adjustResult;
            lastPosition = position;

            count = 0;
        } else {
            ++count;

            flag.setPoint((flag.x * (count - 1) + accAfterFilter.x) / count,
                    (flag.y * (count - 1) + accAfterFilter.y) / count,
                    (flag.z * (count - 1) + accAfterFilter.z) / count);

            if (count > cnt_len) {

                adjustResult = Adjust.adjust(flag, lastRotationAngle);
                velocity.setPoint(0, 0, 0);
                Point3D computationResult = CalculateLAcc.calculateDisplacement(lastVelocity, velocity, timeInterval);
                Point3D position = CalculateLAcc.calculatePosition(lastPosition, computationResult);

                lastTimeStampLinear = timeStamp;
                lastVelocity = velocity;
                lastAdjustResult = adjustResult;
                lastPosition = position;
            }
        }
    }

    private void calRotate() {
        if (!calRotate) {
            if (magneticValues != null && accelerometerValues != null) {
                /**
                 * 根据旋转矩阵计算设备的方位。当它返回时，数组值如下:
                 * values[0]:方位角，绕z轴旋转的角度。这个值表示设备的y轴与磁北极之间的夹角。
                 *           朝北时，这个角度为0，朝南时，这个角度为&pi;同样的，当朝向东方时，
                 *           这个角度是/2，当面向西方时，这个角度是-& /2。取值范围是-&pi;π;。
                 * values[1]:俯仰，绕x轴旋转的角度。这个值表示平行于设备屏幕的平面之间的夹角。
                 *           平行于地面的平面。假设设备的底部边缘面向用户，屏幕是朝上的，将设备
                 *           的顶部边缘向地面倾斜会产生一个正的俯仰角度。取值范围是-&pi;π;。
                 * values[2]:滚，绕y轴旋转角度。这个值表示垂直于设备屏幕的平面与垂直于地面的平
                 *           面之间的夹角。假设该设备的底部边缘面向用户，屏幕是朝上的，将设备的
                 *           左边缘向地面倾斜会产生一个正横倾角。取值范围是-&pi;/2到&pi;/2。
                 *
                 * 将这三种旋转应用于方位角、俯仰、滚动顺序，将一个单位矩阵转化为该方法所传递的旋转矩阵。
                 * 另外，注意三个方向角都用弧度表示。逆时针方向的注意，这是正旋转的标准数学定义，不同意前面
                 * 给出的滚动的定义。
                 *
                 * 在这个轴上以角速度积分，以便从这个样本中得到一个增量的旋转。我们将把这个轴向角的表示转换成四元数，
                 * 然后把它变成旋转矩阵。在实践中，陀螺仪的噪声和偏移会引入一些需要补偿的误差。这通常是使用来自其他
                 * 传感器的信息，但超出了本文档的范围。
                 */
                SensorManager.getRotationMatrix(mR, null, accelerometerValues, magneticValues);
                float[] values = new float[3];
                SensorManager.getOrientation(mR, values);

                lastRotationAngle.x = -(values[1] > 0 ? values[1] : 2 * Math.PI + values[1]);
                lastRotationAngle.y = values[2] > 0 ? values[2] : 2 * Math.PI + values[2];
                lastRotationAngle.z = -(values[0] > 0 ? values[0] : 2 * Math.PI + values[0]);

                mErrorDetectTv.setText(mErrorDetectTv.getText().toString() + "\n初始偏转\nx:"
                        + Math.toDegrees(lastRotationAngle.x) + "\ny:"
                        + Math.toDegrees(lastRotationAngle.y) + "\nz:"
                        + Math.toDegrees(lastRotationAngle.z));
                calRotate = true;
                mSensorManager.unregisterListener(mListener);
            }
        }
    }

    private void initJsSender() {
        if (mSender == null) {
            mSender = new Sender();
            mSender.start();
            Toast.makeText(this, "sender初始化成功", Toast.LENGTH_SHORT).show();
            new Thread(new Runnable() {
                @Override
                public void run() {
                    while (true) {
                        try {
                            mSender.sendDataToJs();
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }
            }).start();
        }
    }
}
